package sensor;

import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.CompassHTSensor;
import lejos.robotics.subsumption.Behavior;
import lejos.util.Delay;
import motor.carrito;

public class USensor {

	private UltrasonicSensor usensor;
	private int[] medidas;
	private int delay;
	private int indice;
	
	public USensor(UltrasonicSensor usensor)
	{
		this.usensor = usensor;
		this.medidas = new int[3];
		this.delay = 75;
		this.indice = 0;
		
		
		this.usensor.setMode(UltrasonicSensor.MODE_PING);
	    this.usensor.ping();
	 	Delay.msDelay(this.delay);
	    int dist = this.usensor.getDistance();
	}
	
	public int getValor()
	{
		this.usensor.ping();
		//Delay.msDelay(this.delay);
		int val = this.usensor.getDistance();
		medidas[2] = medidas[1];
		medidas[1] = medidas[0];
		medidas[0]=val;
		if(val == 255)
		{
			medidas[0] = medidas[1];
			//medidas[0] = medidas[1] + (medidas[1] - medidas[2]);
			//if (medidas[0]<0)
			//{
			//	medidas[0] = 0; 
			//}
		}
		if(indice <= 2){
			indice ++;
		}
//		if(indice > 2)
//		{
//			if(!((medidas[1] > medidas[0]) && (medidas[1] < medidas[2])) ||!((medidas[1] < medidas[0]) && (medidas[1] > medidas[2])))
//		}
		//.println(Integer.toString(medidas[2])+":"+Integer.toString(medidas[1])+":"+Integer.toString(medidas[0]));
		return medidas[0];
		
	}
	
	public boolean detectComeAgent()
	{		
		int dif1 = medidas[2] - medidas[1];
		int dif2 = medidas[1] - medidas[0];
		int dif = Math.abs(dif1 - dif2);
		return (dif1 > 1) && (dif2 > 1) && (dif < 2) && (indice >2);
		//return ((((medidas[2] - medidas[1]) > 1) && ((medidas[1] - medidas[0]) > 1)) && (indice > 2));
	}

	public boolean detectLeaveAgent()
	{
		int dif1 = medidas[1] - medidas[2];
		int dif2 = medidas[0] - medidas[1];
		int dif = Math.abs(dif1 - dif2);
		return (dif1 > 1) && (dif2 > 1) && (dif < 2) && (indice >2);
		//return (((medidas[1] - medidas[2] > 0 )&& (medidas[0] - medidas[1]) > 0) && (indice > 2));
	}
	
	public boolean detectCrossAgent()
	{
		return ((((medidas[2] - medidas[1]) >= 10)&&((medidas[1] - medidas[0]) <= 5)) && ((medidas[1] - medidas[0]) >= 0)  && (indice > 2));
	}
	
	public boolean detectLostAgent()
	{
		//return ((((medidas[0] - medidas[1]) >= 20)||((medidas[1] - medidas[2]) >= 20))  && (indice > 2));
		return (((medidas[1] - medidas[2]) >= 10)&&((medidas[1] - medidas[0]) <= 5) && ((medidas[1] - medidas[0]) >= 0) && (indice > 2));
	}
	
		
	public int getLastDistance()
	{
		return medidas[0];
	}
	
	public void cleanDistances()
	{
		indice = 0;
	}
}
